ORB-SLAM2学习笔记——重定位过程

Relocalization函数

1 、理论部分

2、代码部分

bool Tracking::Relocalization()
{
    // Compute Bag of Words Vector
    //计算当前帧特征点的词袋映射
    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    //获得与当前关键帧相似的候选帧
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
    //候选帧为空，结束
    if(vpCandidateKFs.empty())
        return false;
    //得到候选帧的数量
    const int nKFs = vpCandidateKFs.size();

    // We perform first an ORB matching with each candidate
    // If enough matches are found we setup a PnP solver
    //在候选帧之间进行特征点匹配，若匹配达到一定的数量，进行pnp特征求解
    ORBmatcher matcher(0.75,true);
    //定义个容器来装 pnp求解的结果
    vector<PnPsolver*> vpPnPsolvers;
    //给求解器容器大小与候选帧数量相同
    vpPnPsolvers.resize(nKFs);
    //每个候选关键帧与当前帧的匹配关系
    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nKFs);
    //用于标记要清除的关键帧
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nKFs);

    int nCandidates=0;
    //遍历每个候选关键帧
    for(int i=0; i<nKFs; i++)
    {
        KeyFrame* pKF = vpCandidateKFs[i];
        //如果这个候选关键帧是坏的，则标志为true
        if(pKF->isBad())
            vbDiscarded[i] = true;
        //否则利用词袋进行搜索与当前帧进行匹配
        else
        {
            //匹配点数量
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            //匹配点小于15，匹配失败，还下一个候选关键帧继续
            if(nmatches<15)
            {
                vbDiscarded[i] = true;
                continue;
            }
            //匹配点大于15进入pnp求解
            else
            {
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                //void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
                //概率 最小内点数量 最大迭代次数 最小置位 系数（用于计算最小内点） 阈值
                pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                //求解的结果存入求解容器
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

    // Alternatively perform some iterations of P4P RANSAC
    // Until we found a camera pose supported by enough inliers
    //可适应的调整迭代次数，直到用足够的内点计算出位姿

    //定义个匹配的标识符
    bool bMatch = false;
    //匹配点大于特征点的0.9
    ORBmatcher matcher2(0.9,true);
    //候选关键帧>0并且匹配成功
        while(nCandidates>0 && !bMatch)
    {
        //遍历所有候选关键帧
        for(int i=0; i<nKFs; i++)
        {
            //如果碰到坏帧那么换下一帧继续
            if(vbDiscarded[i])
                continue;

            // Perform 5 Ransac Iterations
            //装内点的容器
            vector<bool> vbInliers;
            //内点个数
            int nInliers;
            //
            bool bNoMore;
            //pnp求解结果
            PnPsolver* pSolver = vpPnPsolvers[i];
            //pnp通过迭代求取位姿
            //cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
            //迭代次数  匹配的一个标识 内点地址 内点数量
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            //如果迭代次数达到最大值或者内点数量不足
            //bNoMore置位,vbDiscarded[i]=true,清除此候选关键帧帧
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If a Camera Pose is computed, optimize
            //如果Pnp计算出了位姿
            if(!Tcw.empty())
            {
                //把这个位姿赋值给当前帧
                Tcw.copyTo(mCurrentFrame.mTcw);
                //成功被再次找到的地图点的集合,其实就是经过RANSAC之后的内点
                set<MapPoint*> sFound;
                //内点的数量
                const int np = vbInliers.size();
                //遍历所有内点
                for(int j=0; j<np; j++)
                {
                    //该位置存在内点
                    if(vbInliers[j])
                    {
                        //提取当前帧与候选关键帧的匹配上的点给地图关键点
                        mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
                        //把这些匹配上的点插入到地图点集合上
                        sFound.insert(vvpMapPointMatches[i][j]);
                    }
                    else
                        //非内点 设为空
                        mCurrentFrame.mvpMapPoints[j]=NULL;
                }
                //优化过程中当前帧好点的数量
                int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                //小于10 还一帧重新来
                if(nGood<10)
                    continue;
                //遍历当前帧所有关键点
                for(int io =0; io<mCurrentFrame.N; io++)
                    //如果为外点那么设置为空
                    if(mCurrentFrame.mvbOutlier[io])
                        mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);

                // If few inliers, search by projection in a coarse window and optimize again
                //如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解，前面的匹配关系是用词袋匹配过程得到的
                //如果内点很少，在更大的窗口下再次搜索
                //如果好点数量小于50 
                if(nGood<50)
                {
                //SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)
                    //SearchByProjection（当前帧 候选关键帧 地图上匹配成功点的集合 新窗口阈值 orb描述子距离）
                    //投影的方法新增好的匹配点的数量
                    int nadditional = matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
                    //如果新增的➕以前的大于等于50
                    if(nadditional+nGood>=50)
                    {
                        //在提取一次好点的数量；记住这是经过位姿优化后的。
                        nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                        // If many inliers but still not enough, search by projection again in a narrower window
                        // the camera has been already optimized with many points
                        //如果好点还不够（差的不是很多），那我们在一个小点的窗口内在搜索一次，因为相机位姿已经得到了优化，
                        //所以我们认为可以缩小范围更细致的搜索，也能看出要求也高了，毕竟是更细致的调整位姿
                        //如果50>好点>30的
                        if(nGood>30 && nGood<50)
                        {
                            //清空地图点集合
                            sFound.clear();
                            //；遍历所有当前帧的所有关键点
                            for(int ip =0; ip<mCurrentFrame.N; ip++)
                                //如果为地图点 也就是好点吧
                                if(mCurrentFrame.mvpMapPoints[ip])
                                    //插入到地图点集合中
                                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                            //SearchByProjection（当前帧 候选关键帧 地图上匹配成功点的集合 新窗口阈值 orb描述子距离）
                            //我们缩小窗口大小和orb描述子距离之后的二次搜索
                            nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);

                            // Final optimization
                            //好点➕二次搜索>=50
                            if(nGood+nadditional>=50)
                            {
                                //提取好点的值
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                                //遍历所有当前帧关键点 ，清除外点
                                for(int io =0; io<mCurrentFrame.N; io++)
                                    if(mCurrentFrame.mvbOutlier[io])
                                        mCurrentFrame.mvpMapPoints[io]=NULL;
                            }//如果还是不能够满足就放弃了
                        }//如果地图点还是比较少的话
                    }//如果通过投影过程获得了比较多的特征点
                }//如果内点较少,那么尝试通过投影关系再次进行匹配


                // If the pose is supported by enough inliers stop ransacs and continue
                //如果好点够了那就这样吧，在爱都曲中人散了吧
                if(nGood>=50)
                {
                    bMatch = true;
                    break;
                }
            }
        }
    }
    //如果失败返回false
    if(!bMatch)
    {
        return false;
    }
    //否则把当前帧的索引号给上一帧，返回true
    else
    {
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }

}//重定位

3、pnp相关代码详解：

epnp代码大全