ORB_SLAM3 LocalMapping中CreateNewMapPoints

这篇具有很好参考价值的文章主要介绍了ORB_SLAM3 LocalMapping中CreateNewMapPoints。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

CreateNewMapPoints新插入的关键帧邻近的关键帧中,通过词袋模型与新插入关键帧中没有匹配的ORB特征进行匹配,并抛弃其中不满足对极约束的匹配点对,接着通过三角化生成地图点

1.获取共视关键帧

GetBestCovisibilityKeyFrames:找出与当前关键帧共视程度最高前nn帧vpNeighKFs

如果是IMU模式,在vpNeighKFs不够nn帧的情况下,将当前关键帧的前n帧prevKF依次添加到vpNeighKFs

    // Retrieve neighbor keyframes in covisibility graph
    int nn = 10;
    // For stereo inertial case
    if(mbMonocular)
        nn=30;
    vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

    if (mbInertial)
    {
        KeyFrame* pKF = mpCurrentKeyFrame;
        int count=0;
        while((vpNeighKFs.size()<=nn)&&(pKF->mPrevKF)&&(count++<nn))
        {
            vector<KeyFrame*>::iterator it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);
            if(it==vpNeighKFs.end())
                vpNeighKFs.push_back(pKF->mPrevKF);
            pKF = pKF->mPrevKF;
        }
    }

2.生成地图点

1.检验vpNeighKFs与当前关键帧的基线:

基线: b a s e l i n e = ∣ O w 2 − O w 1 ∣ < b baseline = \left|Ow_{2} -Ow_{1}\right| < b baseline=Ow2Ow1<b

  • 双目: b a s e l i n e < b baseline < b baseline<b
    • 移动的距离小于基线 b b b,生成的地图点不可靠
  • 单目: r a t i o = b a s e l i n e m e d i a n D e p t h ratio = \frac{ baseline}{medianDepth} ratio=medianDepthbaseline
    • 当前关键帧的地图点深度特别远,基线短,生成的地图点不可靠
        KeyFrame* pKF2 = vpNeighKFs[i];

        GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera;

        // Check first that baseline is not too short
        Eigen::Vector3f Ow2 = pKF2->GetCameraCenter();
        Eigen::Vector3f vBaseline = Ow2-Ow1;
        const float baseline = vBaseline.norm();

        if(!mbMonocular)
        {
            if(baseline<pKF2->mb)
                continue;
        }
        else
        {
            const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
            const float ratioBaselineDepth = baseline/medianDepthKF2;

            if(ratioBaselineDepth<0.01)
                continue;
        }

2.通过Bow和极线约束进行匹配搜索

SearchForTriangulation主要用于LocalMapping生成新的地图点,通过寻找当前关键帧和相邻关键帧中未匹配的特征点之间的匹配关系,主要用Bow来加速搜索,以及极线校验来判断是否能够生成地图点,其输入参数如下:

参数 说明
pkF1 当前关键帧
pkF2 邻近关键帧
vMatchedPairs 匹配关系
bOnlyStereo 双目
bCoarse
  • 计算KF1光心在KF2上的极点:
    c 2 = T c 2 w ⋅ t w c 1 e = π ( c 2 ) \begin{array}{c} c_2 & = & T_{c_{2}w} \cdot t_{wc_1} \\ e & = & \pi \left (c_2\right) \end{array} c2e==Tc2wtwc1π(c2)
        Sophus::SE3f T1w = pKF1->GetPose();
        Sophus::SE3f T2w = pKF2->GetPose();
        Sophus::SE3f Tw2 = pKF2->GetPoseInverse(); // for convenience
        Eigen::Vector3f Cw = pKF1->GetCameraCenter();
        Eigen::Vector3f C2 = T2w * Cw;

        Eigen::Vector2f ep = pKF2->mpCamera->project(C2);
  • 利用Bow的FeatureVector加速搜索
    • 需要KF1和KF2的特征点都未匹配
    • 去除重复匹配vbMatched2[idx2]
    • 描述子距离小于阈值TH_LOW
    • 对于单目,特征点到极点的距离需要小于一定阈值
    • 极线校验:KF2上的特征点到KF1对应极线的距离小于阈值epipolarConstrain
    • 得到最佳的描述子距离bestDist,以及最佳的匹配bestIdx2
        Sophus::SE3f T12;
        Sophus::SE3f Tll, Tlr, Trl, Trr;
        Eigen::Matrix3f R12; // for fastest computation
        Eigen::Vector3f t12; // for fastest computation

        GeometricCamera* pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera;
		//单目
        if(!pKF1->mpCamera2 && !pKF2->mpCamera2){
            T12 = T1w * Tw2;
            R12 = T12.rotationMatrix();
            t12 = T12.translation();
        }
        else{
        //双目
            Sophus::SE3f Tr1w = pKF1->GetRightPose();
            Sophus::SE3f Twr2 = pKF2->GetRightPoseInverse();
            Tll = T1w * Tw2;
            Tlr = T1w * Twr2;
            Trl = Tr1w * Tw2;
            Trr = Tr1w * Twr2;
        }

        Eigen::Matrix3f Rll = Tll.rotationMatrix(), Rlr  = Tlr.rotationMatrix(), Rrl  = Trl.rotationMatrix(), Rrr  = Trr.rotationMatrix();
        Eigen::Vector3f tll = Tll.translation(), tlr = Tlr.translation(), trl = Trl.translation(), trr = Trr.translation();

        // Find matches between not tracked keypoints
        // Matching speed-up by ORB Vocabulary
        // Compare only ORB that share the same node
        int nmatches=0;
        vector<bool> vbMatched2(pKF2->N,false);
        vector<int> vMatches12(pKF1->N,-1);
		//旋转直方图
        vector<int> rotHist[HISTO_LENGTH];
        for(int i=0;i<HISTO_LENGTH;i++)
            rotHist[i].reserve(500);

        const float factor = 1.0f/HISTO_LENGTH;

        DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
        DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
        DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
        DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

        while(f1it!=f1end && f2it!=f2end)
        {
            if(f1it->first == f2it->first)
            {
                for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
                {
                    const size_t idx1 = f1it->second[i1];

                    MapPoint* pMP1 = pKF1->GetMapPoint(idx1);

                    // If there is already a MapPoint skip
                    if(pMP1)
                    {
                        continue;
                    }
					//双目,针孔模型
                    const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0);

                    if(bOnlyStereo)
                        if(!bStereo1)
                            continue;

                    const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1]
                                                                    : (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1]
                                                                                             : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft];
					//为左还是右
                    const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false
                                                                                       : true;

                    const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);

                    int bestDist = TH_LOW;
                    int bestIdx2 = -1;

                    for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
                    {
                        size_t idx2 = f2it->second[i2];

                        MapPoint* pMP2 = pKF2->GetMapPoint(idx2);

                        // If we have already matched or there is a MapPoint skip
                        if(vbMatched2[idx2] || pMP2)
                            continue;
						//双目,针孔
                        const bool bStereo2 = (!pKF2->mpCamera2 &&  pKF2->mvuRight[idx2]>=0);

                        if(bOnlyStereo)
                            if(!bStereo2)
                                continue;

                        const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);

                        const int dist = DescriptorDistance(d1,d2);

                        if(dist>TH_LOW || dist>bestDist)
                            continue;

                        const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]
                                                                        : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]
                                                                                                 : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];
                        //为左还是右                                                    		
                        const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false
                                                                                           : true;
						//单目
                        if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2)
                        {
                            const float distex = ep(0)-kp2.pt.x;
                            const float distey = ep(1)-kp2.pt.y;
                            if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
                            {
                                continue;
                            }
                        }
						//双目
                        if(pKF1->mpCamera2 && pKF2->mpCamera2){
                            if(bRight1 && bRight2){
                                R12 = Rrr;
                                t12 = trr;
                                T12 = Trr;

                                pCamera1 = pKF1->mpCamera2;
                                pCamera2 = pKF2->mpCamera2;
                            }
                            else if(bRight1 && !bRight2){
                                R12 = Rrl;
                                t12 = trl;
                                T12 = Trl;

                                pCamera1 = pKF1->mpCamera2;
                                pCamera2 = pKF2->mpCamera;
                            }
                            else if(!bRight1 && bRight2){
                                R12 = Rlr;
                                t12 = tlr;
                                T12 = Tlr;

                                pCamera1 = pKF1->mpCamera;
                                pCamera2 = pKF2->mpCamera2;
                            }
                            else{
                                R12 = Rll;
                                t12 = tll;
                                T12 = Tll;

                                pCamera1 = pKF1->mpCamera;
                                pCamera2 = pKF2->mpCamera;
                            }

                        }

                        if(bCoarse || pCamera1->epipolarConstrain(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])) // MODIFICATION_2
                        {
                            bestIdx2 = idx2;
                            bestDist = dist;
                        }
                    }

                    if(bestIdx2>=0)
                    {
                        const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[bestIdx2]
                                                                        : (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2]
                                                                                                     : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft];
                        vMatches12[idx1]=bestIdx2;
                        nmatches++;

                        if(mbCheckOrientation)
                        {
                            float rot = kp1.angle-kp2.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(idx1);
                        }
                    }
                }

                f1it++;
                f2it++;
            }
            else if(f1it->first < f2it->first)
            {
                f1it = vFeatVec1.lower_bound(f2it->first);
            }
            else
            {
                f2it = vFeatVec2.lower_bound(f1it->first);
            }
        }

针孔模型:
F 12 = K 1 − T ⋅ t 12 ∧ ⋅ R 12 ⋅ K 2 − 1 F_{12} = K_1^{-T}\cdot t_{12}^{\wedge}\cdot R_{12}\cdot K_{2}^{-1} F12=K1Tt12R12K21

l = x 1 T ⋅ F 12 l = x_{1}^T\cdot F_{12} l=x1TF12

d = a ∗ a + b ∗ b a x 2 + b y 2 + c d=\frac{a*a + b*b}{ax_2+by_2+c} d=ax2+by2+caa+bb

  • 利用旋转直方图,检测方向一致性
        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++)
            {
                if(i==ind1 || i==ind2 || i==ind3)
                    continue;
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    vMatches12[rotHist[i][j]]=-1;
                    nmatches--;
                }
            }

        }

        vMatchedPairs.clear();
        vMatchedPairs.reserve(nmatches);

        for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
        {
            if(vMatches12[i]<0)
                continue;
            vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
        }

3.对于每一对匹配对校验并三角化

  • 检查视差角是否满足要求
    cosParallaxRays
    c o s θ = ( R w c 1 ⋅ x 1 ) ⋅ ( R w c 2 ⋅ x 2 ) ∣ R w c 1 ⋅ x 1 ∣ ⋅ ∣ R w c 2 ⋅ x 2 ∣ cos\theta = \frac{\left(R_{wc1}\cdot x_1\right)\cdot \left(R_{wc2}\cdot x_2\right)}{\left|R_{wc1}\cdot x_1\right|\cdot \left|R_{wc2}\cdot x_2\right|} cosθ=Rwc1x1Rwc2x2(Rwc1x1)(Rwc2x2)
    cosParallaxStereo
    c o s ( 2 arctan ⁡ ( b 2 d ) ) cos\left(2 \arctan\left(\frac{\frac{b}{2}}{d}\right)\right) cos(2arctan(d2b))

  • 通过三角化对每对匹配点生成新的3D点

  • 检查3D点是否在相机前方: z > 0 z > 0 z>0

  • 判断3D点在关键帧下的重投影误差

  • 检查尺度是否合理: d i s t 1 d i s t 2 ≈ σ 1 σ 2 \frac{dist_1}{dist_2}\approx \frac{\sigma _{1}}{\sigma _{2}} dist2dist1σ2σ1

  • 将3D点生成地图点,并添加相应属性

  • 将该地图点添加到地图中

  • 将该地图点添加到mlpRecentAddedMapPoints中,经过MapPointCulling函数的检验文章来源地址https://www.toymoban.com/news/detail-790620.html

        Sophus::SE3<float> sophTcw2 = pKF2->GetPose();
        Eigen::Matrix<float,3,4> eigTcw2 = sophTcw2.matrix3x4();
        Eigen::Matrix<float,3,3> Rcw2 = eigTcw2.block<3,3>(0,0);
        Eigen::Matrix<float,3,3> Rwc2 = Rcw2.transpose();
        Eigen::Vector3f tcw2 = sophTcw2.translation();

        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
        const int nmatches = vMatchedIndices.size();
        for(int ikp=0; ikp<nmatches; ikp++)
        {
            const int &idx1 = vMatchedIndices[ikp].first;
            const int &idx2 = vMatchedIndices[ikp].second;

            const cv::KeyPoint &kp1 = (mpCurrentKeyFrame -> NLeft == -1) ? mpCurrentKeyFrame->mvKeysUn[idx1]
                                                                         : (idx1 < mpCurrentKeyFrame -> NLeft) ? mpCurrentKeyFrame -> mvKeys[idx1]
                                                                                                               : mpCurrentKeyFrame -> mvKeysRight[idx1 - mpCurrentKeyFrame -> NLeft];
            const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
            //双目
            bool bStereo1 = (!mpCurrentKeyFrame->mpCamera2 && kp1_ur>=0);
            //右
            const bool bRight1 = (mpCurrentKeyFrame -> NLeft == -1 || idx1 < mpCurrentKeyFrame -> NLeft) ? false
                                                                                                         : true;

            const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]
                                                            : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]
                                                                                     : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];

            const float kp2_ur = pKF2->mvuRight[idx2];
            //双目
            bool bStereo2 = (!pKF2->mpCamera2 && kp2_ur>=0);
            //右
            const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false
                                                                               : true;

            if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){
                if(bRight1 && bRight2){
                    sophTcw1 = mpCurrentKeyFrame->GetRightPose();
                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();

                    sophTcw2 = pKF2->GetRightPose();
                    Ow2 = pKF2->GetRightCameraCenter();

                    pCamera1 = mpCurrentKeyFrame->mpCamera2;
                    pCamera2 = pKF2->mpCamera2;
                }
                else if(bRight1 && !bRight2){
                    sophTcw1 = mpCurrentKeyFrame->GetRightPose();
                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();

                    sophTcw2 = pKF2->GetPose();
                    Ow2 = pKF2->GetCameraCenter();

                    pCamera1 = mpCurrentKeyFrame->mpCamera2;
                    pCamera2 = pKF2->mpCamera;
                }
                else if(!bRight1 && bRight2){
                    sophTcw1 = mpCurrentKeyFrame->GetPose();
                    Ow1 = mpCurrentKeyFrame->GetCameraCenter();

                    sophTcw2 = pKF2->GetRightPose();
                    Ow2 = pKF2->GetRightCameraCenter();

                    pCamera1 = mpCurrentKeyFrame->mpCamera;
                    pCamera2 = pKF2->mpCamera2;
                }
                else{
                    sophTcw1 = mpCurrentKeyFrame->GetPose();
                    Ow1 = mpCurrentKeyFrame->GetCameraCenter();

                    sophTcw2 = pKF2->GetPose();
                    Ow2 = pKF2->GetCameraCenter();

                    pCamera1 = mpCurrentKeyFrame->mpCamera;
                    pCamera2 = pKF2->mpCamera;
                }
                eigTcw1 = sophTcw1.matrix3x4();
                Rcw1 = eigTcw1.block<3,3>(0,0);
                Rwc1 = Rcw1.transpose();
                tcw1 = sophTcw1.translation();

                eigTcw2 = sophTcw2.matrix3x4();
                Rcw2 = eigTcw2.block<3,3>(0,0);
                Rwc2 = Rcw2.transpose();
                tcw2 = sophTcw2.translation();
            }

            // Check parallax between rays
            Eigen::Vector3f xn1 = pCamera1->unprojectEig(kp1.pt);
            Eigen::Vector3f xn2 = pCamera2->unprojectEig(kp2.pt);

            Eigen::Vector3f ray1 = Rwc1 * xn1;
            Eigen::Vector3f ray2 = Rwc2 * xn2;
            const float cosParallaxRays = ray1.dot(ray2)/(ray1.norm() * ray2.norm());

            float cosParallaxStereo = cosParallaxRays+1;
            float cosParallaxStereo1 = cosParallaxStereo;
            float cosParallaxStereo2 = cosParallaxStereo;

            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]));

            if (bStereo1 || bStereo2) totalStereoPts++;
            
            cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);

            Eigen::Vector3f x3D;

            bool goodProj = false;
            bool bPointStereo = false;
            if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 ||
                                                                          (cosParallaxRays<0.9996 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial)))
            {
                goodProj = GeometricTools::Triangulate(xn1, xn2, eigTcw1, eigTcw2, x3D);
                if(!goodProj)
                    continue;
            }
            else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
            {
                countStereoAttempt++;
                bPointStereo = true;
                goodProj = mpCurrentKeyFrame->UnprojectStereo(idx1, x3D);
            }
            else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
            {
                countStereoAttempt++;
                bPointStereo = true;
                goodProj = pKF2->UnprojectStereo(idx2, x3D);
            }
            else
            {
                continue; //No stereo and very low parallax
            }

            if(goodProj && bPointStereo)
                countStereoGoodProj++;

            if(!goodProj)
                continue;

            //Check triangulation in front of cameras
            float z1 = Rcw1.row(2).dot(x3D) + tcw1(2);
            if(z1<=0)
                continue;

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

            //Check reprojection error in first keyframe
            const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
            const float x1 = Rcw1.row(0).dot(x3D)+tcw1(0);
            const float y1 = Rcw1.row(1).dot(x3D)+tcw1(1);
            const float invz1 = 1.0/z1;

            if(!bStereo1)
            {
                cv::Point2f uv1 = pCamera1->project(cv::Point3f(x1,y1,z1));
                float errX1 = uv1.x - kp1.pt.x;
                float errY1 = uv1.y - kp1.pt.y;

                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;
                if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
                    continue;
            }

            //Check reprojection error in second keyframe
            const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
            const float x2 = Rcw2.row(0).dot(x3D)+tcw2(0);
            const float y2 = Rcw2.row(1).dot(x3D)+tcw2(1);
            const float invz2 = 1.0/z2;
            if(!bStereo2)
            {
                cv::Point2f uv2 = pCamera2->project(cv::Point3f(x2,y2,z2));
                float errX2 = uv2.x - kp2.pt.x;
                float errY2 = uv2.y - 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
            Eigen::Vector3f normal1 = x3D - Ow1;
            float dist1 = normal1.norm();

            Eigen::Vector3f normal2 = x3D - Ow2;
            float dist2 = normal2.norm();

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

            if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints)) // MODIFICATION
                continue;

            const float ratioDist = dist2/dist1;
            const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];

            if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
                continue;

            // Triangulation is succesfull
            MapPoint* pMP = new MapPoint(x3D, mpCurrentKeyFrame, mpAtlas->GetCurrentMap());
            if (bPointStereo)
                countStereo++;
            
            pMP->AddObservation(mpCurrentKeyFrame,idx1);
            pMP->AddObservation(pKF2,idx2);

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

            pMP->ComputeDistinctiveDescriptors();

            pMP->UpdateNormalAndDepth();

            mpAtlas->AddMapPoint(pMP);
            mlpRecentAddedMapPoints.push_back(pMP);
        }

到了这里,关于ORB_SLAM3 LocalMapping中CreateNewMapPoints的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 在realsense D455相机上运行orb_slam3

    参考https://blog.csdn.net/weixin_42990464/article/details/133019718?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522171109916816777224423276%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257Drequest_id=171109916816777224423276biz_id=0utm_medium=distribute.pc_search_result.none-task-blog-2 blog first_rank_ecpm_v1~rank_v31_ecpm

    2024年03月26日
    浏览(56)
  • RGB-L:基于激光雷达增强的ORB_SLAM3(已开源)

    点云PCL免费知识星球,点云论文速读。 文章:RGB-L: Enhancing Indirect Visual SLAM using LiDAR-based Dense Depth Maps 作者:Florian Sauerbeck, Benjamin Obermeier, Martin Rudolph 编辑:点云PCL 代码:https://github.com/TUMFTM/ORB_SLAM3_RGBL.git 欢迎各位加入免费知识星球,获取PDF论文,欢迎转发朋友圈。文章仅

    2024年02月07日
    浏览(48)
  • orb_slam3实现保存/加载地图功能and发布位姿功能

    先说方法:在加载的相机参数文件.yaml的最前面加上下面两行就行。 第一行表示从本地加载名为\\\"MH01_to_MH05_stereo_inertial.osa\\\"的地图文件,第二行表示保存名为\\\"MH01_to_MH05_stereo_inertial.osa\\\"的地图到本地。第一次运行建图时注释掉第一行,只使用第二行,加载地图重定位时反过来,

    2024年02月15日
    浏览(45)
  • ORB_SLAM3 ROS编译及使用D435I运行

    本文介绍ORB_SLAM3编译、运行中遇到问题,尤其涉及到ORB_SLAM3 ROS编译遇到的问题。最后通过使用D435I完成在室内环境下运行。 本文运行环境在Ubuntu20.04 + ROS noetic。 一、ORB_SLAM3 依赖安装 ORB_SLAM3 依赖的安装,有同学喜欢上来就baidu,按照别人介绍的安装,这样做大多数时候会出现

    2024年02月03日
    浏览(51)
  • ORB_SLAM2配置——基于Ubuntu20.04+ROS+gazebo仿真

    一、引言 ORB-SLAM2,它是基于单目、双目或RGB-D相机的一个完整的SLAM系统,其中包括地图重用、回环检测和重定位功能。这个系统可以适用于多种环境,无论是室内小型手持设备,还是工厂环境中飞行的无人机和城市中行驶的车辆,其都可以在标准CPU上实时运行。该系统的后端

    2023年04月13日
    浏览(49)
  • ORB_SLAM3启动流程以stereo_inertial_realsense_D435i为例

    概述 ORB-SLAM3 是第一个同时具备纯视觉(visual)数据处理、视觉+惯性(visual-inertial)数据处理、和构建多地图(multi-map)功能,支持单目、双目以及 RGB-D 相机,同时支持针孔相机、鱼眼相机模型的 SLAM 系统。 主要创新点: 1.在 IMU 初始化阶段引入 MAP。该初始化方法可以实时

    2024年02月12日
    浏览(47)
  • ORB_SLAM3配置及修改——将图像、点云用ROS消息发布(基于无人机仿真)

            本文有点长,可以根据目录跳转到想看的部分。因为仿真和应用环境不同,可能例程的运行方式(输入话题等)有所不同,但第三部分有关ORB_SLAM3相机仿真标定、第四部分有关ORB_SLAM3源码修改的部分是通用的。 目录 一、仿真环境配置 1.双系统安装 ① 工具准备 ②

    2024年04月10日
    浏览(50)
  • ORB_SLAM2算法中如何计算右目和左目两个特征点的是否匹配?

    在进行特征匹配时使用的循环。 vCandidates 是

    2024年02月06日
    浏览(44)
  • 记录:ubuntu20.04+ORB_SLAM2_with_pointcloud_map+ROS noetic

    由于相机实时在线运行需要ROS,但Ubuntu22.04只支持ROS2,于是重装Ubuntu20.04。 上一篇文章跑通的是官方版本的ORB_SLAM2,不支持点云显示。高翔修改版本支持RGB-D相机的点云显示功能。 高翔修改版本ORB_SLAM2:https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map 环境:ubunntu20.04、opencv3.4.

    2024年02月11日
    浏览(57)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包