ORB_SLAM2原理及代码解析：Tracking::CreateInitialMapMonocular() 函数

WWZZ20252025-10-06 1:39

目录

[1 作用](#1 作用)

[2 参数及含义](#2 参数及含义)

[3 代码](#3 代码)

[4 解析](#4 解析)

[4.1 创建关键帧](#4.1 创建关键帧)

[4.2 计算词袋](#4.2 计算词袋)

[4.2.1 Frame::ComputeBoW()](#4.2.1 Frame::ComputeBoW())

[4.2.2 KeyFrame::ComputeBoW()](#4.2.2 KeyFrame::ComputeBoW())

[4.3 插入关键帧到地图](#4.3 插入关键帧到地图)

[4.4 创建初始MapPoints](#4.4 创建初始MapPoints)

[4.5 建立关键帧与地图点的双向关系](#4.5 建立关键帧与地图点的双向关系)

[4.6 优化MapPoint属性](#4.6 优化MapPoint属性)

[4.7 在当前帧中更新关联关系](#4.7 在当前帧中更新关联关系)

[4.8 更新关键帧之间连接](#4.8 更新关键帧之间连接)

[4.8.1 UpdateConnections() 函数](#4.8.1 UpdateConnections() 函数)

[4.9 全局BA](#4.9 全局BA)

[4.9.1GlobalBundleAdjustemnt() 函数](#4.9.1GlobalBundleAdjustemnt() 函数)

[4.10 设置尺度](#4.10 设置尺度)

[4.10.1 ComputeSceneMedianDepth() 函数](#4.10.1 ComputeSceneMedianDepth() 函数)

[4.11 更新地图解结构与状态](#4.11 更新地图解结构与状态)

[4.11.1 GetMapPointMatches](#4.11.1 GetMapPointMatches)

[4.11.2 InsertKeyFrame() 函数](#4.11.2 InsertKeyFrame() 函数)

1 作用

在单目 SLAM 初始化完成后（即估计出了两帧之间的姿态 R、t 和三维点mvIniP3D），

该函数作用：

（1）创建两个关键帧（初始帧 + 当前帧）；

（2）用匹配关系创建初始地图点；

（3）建立关键帧之间的观测关系；

（4）执行一次全局 BA（Bundle Adjustment）优化；

（5）规范化尺度（因为单目无法直接估计绝对尺度）；

（6）将地图数据交给 LocalMapping 线程进行后续优化。

2 参数及含义

3 代码

复制代码 
void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);


    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // Insert KFs in the map
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // Create MapPoints and asscoiate to keyframes
    for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        cv::Mat worldPos(mvIniP3D[i]);

        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);

        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

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

    // Update Connections
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // Bundle Adjustment
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    // Set median depth to 1
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Scale initial baseline
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    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;
}

4 解析

4.1 创建关键帧
复制代码 
// Create KeyFrames
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
4.2 计算词袋
复制代码 
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

这边词袋计算调用的是KeyFrame.cc，关键帧词袋运算。

4.2.1 Frame::ComputeBoW()

（1）声明：Frame.h

复制代码 
    // Compute Bag of Words representation.
    void ComputeBoW();

（2）定义：Frame.cc

复制代码 
void Frame::ComputeBoW()
{
    if(mBowVec.empty())
    {
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

（3）举例

4.2.2 KeyFrame::ComputeBoW()

（1）声明：KeyFrame.h

复制代码 
    // Bag of Words Representation
    void ComputeBoW();

（2）定义：KeyFrame.cc

复制代码 
void KeyFrame::ComputeBoW()
{
    if(mBowVec.empty() || mFeatVec.empty())
    {
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        // Feature vector associate features with nodes in the 4th level (from leaves up)
        // We assume the vocabulary tree has 6 levels, change the 4 otherwise
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

PS.KeyFrame::ComputeBoW() 计算结果用于插入关键帧、回环检测时，存入关键帧数据库DB；Frame::ComputeBoW() 计算结果用于初始化、更踪中，结果不存入地图。

4.3 插入关键帧到地图
复制代码 
    // Insert KFs in the map
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);
4.4 创建初始MapPoints
复制代码 
    // Create MapPoints and asscoiate to keyframes
    for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        cv::Mat worldPos(mvIniP3D[i]);

        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
4.5 建立关键帧与地图点的双向关系
复制代码 
        //初始关键帧的第 i 个关键点，对应的是地图点 pMP
        pKFini->AddMapPoint(pMP,i);
        //当前帧的第 mvIniMatches[i] 个关键点，也对应这个同一个 3D 地图点
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        //这个地图点 pMP 在关键帧 pKFini 中被第 i 个特征点观测到了
        pMP->AddObservation(pKFini,i);
        //这个地图点 pMP 在当前帧 pKFcur 中被第 mvIniMatches[i] 个特征点观测到了
        pMP->AddObservation(pKFcur,mvIniMatches[i]);
4.6 优化MapPoint属性
复制代码 
        //给这个地图点计算一个 "代表性 ORB 描述符"
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

（1）ComputeDistinctiveDescriptors() 函数详见：

https://blog.csdn.net/weixin_45728280/article/details/152320324?spm=1001.2014.3001.5501

（2）UpdateNormalAndDepth() 函数详见：

https://blog.csdn.net/weixin_45728280/article/details/152320646?spm=1001.2014.3001.5501

4.7 在当前帧中更新关联关系
复制代码 
        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        //当前帧各个点都不是异常点，后续重投影误差较大时会被标记为True，即外点
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        //把刚生成的 MapPoint 加入到全局地图 (Map) 中的地图点容器
        mpMap->AddMapPoint(pMP);
    }
4.8 更新关键帧之间连接
复制代码 
    // Update Connections
    //根据当前关键帧与其他关键帧之间共同观测到的MapPoints数量，建立关键帧间的连接关系
    //关键帧共视图、最小生成树、关键帧连接权重
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();
4.8.1 UpdateConnections() 函数

（1）声明：KeyFrame.h

复制代码 
void UpdateConnections();

（2）定义：KeyFrame.cc

复制代码 
void KeyFrame::UpdateConnections()
{
    map<KeyFrame*,int> KFcounter;

    vector<MapPoint*> vpMP;

    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    //For all map points in keyframe check in which other keyframes are they seen
    //Increase counter for those keyframes
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        if(!pMP)
            continue;

        if(pMP->isBad())
            continue;

        map<KeyFrame*,size_t> observations = pMP->GetObservations();

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            if(mit->first->mnId==mnId)
                continue;
            KFcounter[mit->first]++;
        }
    }

    // This should not happen
    if(KFcounter.empty())
        return;

    //If the counter is greater than threshold add connection
    //In case no keyframe counter is over threshold add the one with maximum counter
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;

    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }
        if(mit->second>=th)
        {
            vPairs.push_back(make_pair(mit->second,mit->first));
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    if(vPairs.empty())
    {
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        if(mbFirstConnection && mnId!=0)
        {
            mpParent = mvpOrderedConnectedKeyFrames.front();
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }

    }
}
4.9 全局BA
复制代码 
    // Bundle Adjustment
    //输出：New Map created with XXX points
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

    Optimizer::GlobalBundleAdjustemnt(mpMap,20);
4.9.1GlobalBundleAdjustemnt() 函数

（1）声明：

复制代码 
    void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL,
                                       const unsigned long nLoopKF=0, const bool bRobust = true);

（2）定义：

复制代码 
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
    BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}

void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    vector<bool> vbNotIncludedMP;
    vbNotIncludedMP.resize(vpMP.size());

    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    long unsigned int maxKFid = 0;

    // Set KeyFrame vertices
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        vSE3->setId(pKF->mnId);
        vSE3->setFixed(pKF->mnId==0);
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

    const float thHuber2D = sqrt(5.99);
    const float thHuber3D = sqrt(7.815);

    // Set MapPoint vertices
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

       const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        int nEdges = 0;
        //SET EDGES
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {

            KeyFrame* pKF = mit->first;
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;

            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            if(pKF->mvuRight[mit->second]<0)
            {
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuber2D);
                }

                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;

                optimizer.addEdge(e);
            }
            else
            {
                Eigen::Matrix<double,3,1> obs;
                const float kp_ur = pKF->mvuRight[mit->second];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuber3D);
                }

                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                e->bf = pKF->mbf;

                optimizer.addEdge(e);
            }
        }

        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }

    // Optimize!
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

    // Recover optimized data

    //Keyframes
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        if(nLoopKF==0)
        {
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }
        else
        {
            pKF->mTcwGBA.create(4,4,CV_32F);
            Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
            pKF->mnBAGlobalForKF = nLoopKF;
        }
    }

    //Points
    for(size_t i=0; i<vpMP.size(); i++)
    {
        if(vbNotIncludedMP[i])
            continue;

        MapPoint* pMP = vpMP[i];

        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));

        if(nLoopKF==0)
        {
            pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
            pMP->UpdateNormalAndDepth();
        }
        else
        {
            pMP->mPosGBA.create(3,1,CV_32F);
            Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
            pMP->mnBAGlobalForKF = nLoopKF;
        }
    }

}

（1）BA优化中，相机位姿顶点是世界坐标系下，初值用 PnP / 本质矩阵 / 深度得到；地图顶点是世界坐标系下的三维点​，初值通过相机位姿 + 特征点匹配三角化得到。

（2）BA优化中投影误差计算如下：

深度问题在使用RGB-D的时候可以解决，也就不用这种误差测算；单目的时候因为没有深度值，需要根据像素坐标进行三角化得到世界坐标，但这个世界坐标是估计值，所以需要重投影到像素坐标下与实际的像素坐标进行最小化。

4.10 设置尺度
复制代码 
    // Set median depth to 1
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;
4.10.1 ComputeSceneMedianDepth() 函数

（1）声明：KeyFrame.h

复制代码 
    // Compute Scene Depth (q=2 median). Used in monocular.
    float ComputeSceneMedianDepth(const int q);

（2）定义：KeyFrame.cc

复制代码 
float KeyFrame::ComputeSceneMedianDepth(const int q)
{
    vector<MapPoint*> vpMapPoints;
    cv::Mat Tcw_;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPose);
        vpMapPoints = mvpMapPoints;
        Tcw_ = Tcw.clone();
    }

    vector<float> vDepths;
    vDepths.reserve(N);
    cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
    Rcw2 = Rcw2.t();
    float zcw = Tcw_.at<float>(2,3);
    for(int i=0; i<N; i++)
    {
        if(mvpMapPoints[i])
        {
            MapPoint* pMP = mvpMapPoints[i];
            cv::Mat x3Dw = pMP->GetWorldPos();
            float z = Rcw2.dot(x3Dw)+zcw;
            vDepths.push_back(z);
        }
    }

    sort(vDepths.begin(),vDepths.end());

    return vDepths[(vDepths.size()-1)/q];
}
4.11 更新地图解结构与状态
复制代码 
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Scale initial baseline，对平移进行归一化，旋转不用
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    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;
}
4.11.1 GetMapPointMatches

（1）声明：

复制代码 
    std::vector<MapPoint*> GetMapPointMatches();

（2）定义：

复制代码 
vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints;
}
4.11.2 InsertKeyFrame() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152416212?spm=1001.2014.3001.5501

相关推荐
WWZZ20252 小时前
ORB_SLAM2原理及代码解析：Tracking::MonocularInitialization() 函数
人工智能·opencv·算法·计算机视觉·机器人·感知·单目相机
那雨倾城2 小时前
PiscCode:基于OpenCV的前景物体检测
图像处理·python·opencv·计算机视觉
何须至远2 小时前
机器人市场：犹如一颗深水核弹
stm32·单片机·机器人
eve杭3 小时前
解锁数据主权与极致性能：AI本地部署的全面指南
大数据·人工智能·5g·ai
幽络源小助理3 小时前
基于Napcat+Koshi的QQ群AI大模型机器人部署-幽络源
机器人·napcat·koshi·qqai机器人
数字时代全景窗3 小时前
商业航天与数字经济（一）：从4G、5G得与失，看6G时代商业航天如何成为新经济引擎？
大数据·人工智能·5g
L_09074 小时前
【Algorithm】双指针算法与滑动窗口算法
c++·算法
F_D_Z4 小时前
【一文理解】下采样与上采样区别
人工智能·深度学习·计算机视觉
CiLerLinux4 小时前
第三十五章 ESP32S3 摄像头实验
图像处理·人工智能·计算机视觉
热门推荐
01HarmonyOS NEXT开发进阶（十四）：HarmonyOS应用开发者基础认证试题集汇总及答案解析02KGG转MP3工具|非KGM文件|解密音频03UV安装并设置国内源04jdk21下载、安装（Windows、Linux、macOS）05GitHub 镜像站点0646个Nano-banana 精选提示词，持续更新中07Linux下V2Ray安装配置指南08零基础搭建赛博朋克个人主页：蓝耘Claude Code完整实战教程09最新b站加密关键字段的逆向（视频和评论爬取）10MIUI显示/隐藏5G开关的方法，信号弱时开启手机Wifi通话方法