重定位解析与思考

Posted 2023-02-05

0. 简介

SLAM算法在加入回环检测与修正后，效果会显著提高。而很多人就会思考，有没有一个比较稳健的方法，能够让机器人跟丢之后重新找回当前的姿态。虽然二者目的不同，重定位主要为了恢复姿态估计，而回环为了解决飘移，提高全局精度。但是二者在匹配帧上可以共享一些算法。

1. 视觉重定位回顾

比如说对于单目VSLAM算法用的比较多的还是基于​​BoW​​的匹配方案（ORB-SLAM，VINS等），也有基于机器学习匹配patch的方法（PTAM）。同时也有一些简单粗暴的解决方案，比如单独开线程暴力匹配之前所有关键帧（LSD），或者只扰动初始值不断与上一个关键帧进行匹配（DSO）。下图为ORB-SLAM的重定位的思路，我们可以看到主流的特征点方法是查询关键帧数据库进行快速查找与当前帧相似的候选重定位帧。这也是图像特征的优势

bool Tracking::Relocalization()

    // Compute Bag of Words Vector
    // 步骤1：计算当前帧特征点的Bow映射，能够得到当前帧的词袋向量以及featureVector
    // 可用于SearchByBoW寻找匹配特征点
    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // 步骤2：找到与当前帧相似的候选关键帧，
    // 这里会通过查询关键帧数据库进行快速查找与当前帧相似的候选重定位帧vpCandidateKFs
    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
    ORBmatcher matcher(0.75,true);
    
    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];
        if(pKF->isBad())
            vbDiscarded[i] = true;
        else
        
            // 步骤3：通过BoW进行匹配
            // 利用SearchByBoW查找当前帧与关键帧的匹配点vvpMapPointMatches
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            // 如果匹配点数小于15个点，跳过
            if(nmatches<15)
            
                vbDiscarded[i] = true;
                continue;
            
            // 如果匹配点数大于15个点，建立当前帧与关键帧之间的PNP求解器；
            // 仅仅如建立这个求解器，还未求解
            else
            
                // 初始化PnPsolver
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                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;
    ORBmatcher matcher2(0.9,true);

    // 如果候选关键帧数大于0且没有重定位成功
    while(nCandidates>0 && !bMatch)
    
        for(int i=0; i<nKFs; i++)
        
            // 若当前候选关键帧与当前帧匹配数量小于15，跳过
            if(vbDiscarded[i])
                continue;

            // Perform 5 Ransac Iterations
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;

            // 步骤4：通过EPnP算法估计初始位姿
            PnPsolver* pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            // 若RANSAC失败，当前候选关键帧被提出候选帧
            if(bNoMore)
            
                vbDiscarded[i]=true;
                nCandidates--;
            

            // If a Camera Pose is computed, optimize
            // PNP求解出了一个比较初始的位姿，比较粗糙，需要进一步优化
            if(!Tcw.empty())
            
                // 把刚刚PNP求解的位姿赋给当前帧位姿
                Tcw.copyTo(mCurrentFrame.mTcw);

                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;
                

                // 步骤5：通过PoseOptimization对姿态进行优化求解
                int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                // 内点小于10，跳过
                if(nGood<10)
                    continue;
                // 刚才的PO优化会滤除一些外点
                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
                // 步骤6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
                // 作者认为10<=nGood<50时仍有可能重定位成功，由于PO调整了位姿，
                // 可以通过位姿投影的方式将候选关键帧上的地图点投影在当前帧上进行搜索匹配点，
                // 从而增加匹配，然后再优化以得到足够多的内点
                if(nGood<50)
                
                    int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
                    // 新增的点与之前PO内点之和大于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
                        // 不够多呀，不要放弃，再来一遍
                        if(nGood>30 && nGood<50)
                        
                            sFound.clear();
                            for(int ip =0; ip<mCurrentFrame.N; ip++)
                                if(mCurrentFrame.mvpMapPoints[ip])
                                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                            nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);

                            // Final optimization
                            // 最后一次优化啦~
                            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
                // 只要找到一个候选关键帧与当前帧的匹配点数大于50就重定位成功！
                if(nGood>=50)
                
                    bMatch = true;
                    break;
                
            
        
    

    if(!bMatch)
    
        return false;
    
    else
    
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    

2. 激光重定位回顾

对于激光雷达而言，一般方法除了遍历比较法也就是通过​​ICP​​,NDT完成帧间比较外，还有使用​​SC​​，​​Iris​​这些常用的回环检测方法都可以用来适配重定位。此外还有使用​​SegMatch​​​、​​SegＭap​​​、​​LOL​​、​​OverlapNet​​这类深度学习处理方法。

相较于视觉通过词袋模型这类方式能够满足快速的匹配，虽然激光雷达也有SC这类压缩激光特征的方法，但是实际上这类方法仍然无法满足我们的需求，因为精度不高，所以我们可以将，激光的位置识别主要有两种提特征的方式： local feature 与global feature。

1. local feature主要还是提取特征点与匹配。这样提的特征做位置识别，由于特征是局部的，如果直接对整张地图使用local feature，其匹配的成功率会比较低。
2. global feature是指对于输入的整个点云，分析点云的一些特征并抽象出来，比如得到一个关于点云的直方图（SC和SegMatch思想），再拿直方图去匹配。这样做的缺点是，机器人的位置稍作变化，输入点云的global feature就可能不一样。即global feature的一致性比较差。这里Iris的工作做了一定的改善，但是仍然需要加入local feature完成精匹配。