一文詳解SLAM回環及優化

作者丨lovely_yoshino

0. 前言

随着路徑的不斷延伸，機器人的建圖過程會存在不斷地累計誤差。而傳統的以gmapping為代表的使用粒子濾波進行定位的slam建圖方式。以及ORB-SLAM為代表包含的局部優化和全局優化來調整外。但是這些處理方式隻能減緩誤差累計的程度，無法消除，而現在最為常用消除累計誤差的方法就是利用回環檢測來優化位姿。

當新的關鍵幀加入到優化模型時，在關鍵幀附近進行一次局部優化。

在全局優化中，所有的關鍵幀（除了第一幀）和三維點都參與優化。

1. 回環檢測

回環檢測作為近年來slam行業必備的部分，是指機器人識别曾到達某場景，使得地圖閉環的能力。

回環檢測之是以能成為一個難點，是因為：如果回環檢測成功，可以顯著地減小累積誤差，幫助機器人更精準、快速的進行避障導航工作。而錯誤的檢測結果可能使地圖變得很糟糕。是以，回環檢測在大面積、大場景地圖建構上是非常有必要的 。

詞袋模型(bag of words,BoW)早期是一種文本表征方法，後引入到計算機視覺領域，逐漸成為一種很有效的圖像特征模組化方法。它通過提取圖像特征，再将特征進行分類建構視覺字典，然後采用視覺字典中的單詞集合可以表征任一幅圖像。

換句話說，通過BOW可以把一張圖檔表示成一個向量。這對判斷圖像間的關聯很有幫助，是以目前比較流行的回環解決方案都是采用的BoW及其基礎上衍生的算法IAB-MAP、FAB-MAP是在濾波架構下計算回環機率，RTAB-MAP采用關鍵幀比較相似性，DLoopDetector（在DBoW2基礎上開發的回環檢測庫）采用連續幀的相似性檢測判斷是否存在回環。回環檢測主要由BoW子產品、算法子產品、驗證子產品三部分組成。

詞袋模型（BoW子產品）

每一幀都可以用單詞來描述，也就是這一幀中有哪些單詞，這裡隻關心了有沒有，而不必關心具體在哪裡。隻有兩幀中單詞種類相近才可能構成回環。

（ａ）圖像預處理，假設訓練集有Ｍ幅圖像，将圖像标準化為patch，統一格式和規格;

（ｂ）特征提取，假設Ｍ幅圖像，對每一幅圖像提取特征，共提取出Ｎ個SIFT特征;

（ｃ）特征聚類，采用K-means算法把N個對象分為K個簇 （視覺單詞表），使簇内具有較高的相似度，而簇間相似度較低；

（ｄ）統計得到圖像的碼本，每幅圖像以單詞表為規範對該幅圖像的每一個SIFT特征點計算它與單詞表中每個單詞的距 離，最近的加1，便得到該幅圖像的碼本；還需要碼本矢量歸一化，因為每一幅圖像的SIFT特征個數不定，是以需要歸一化。

相似度計算（算法子產品）

現在有了字典，但還有一點需要注意。我們利用單詞表示圖像，目的是發現回環，也就是兩幀圖像具有較高的相似性。那其實不同的單詞對這一目的的貢獻性是不同的。例如，我這篇文章中有很多“我們”這個詞，但這個詞并不能告訴我們太多資訊。

而有些單詞例如“回環”、“K-means”就比較具有代表性，能大概告訴我們這句話講了什麼。是以不同的單詞應該具有不同的權重。

（ａ）貝葉斯估計方法。采用BoW描述機器人每一位置的場景圖像，估計已擷取圖像與對應位置的先驗機率，對目前時刻計算該新場景圖像與已通路位置比對的後驗機率，機率大于門檻值則标記為閉環。

（ｂ）相似性方法。有了字典以後，給定任意特征點f_i，隻要在字典樹中逐層查找，最後都能找到與之對應的單詞w_i。通常字典足夠大，可以說它們來自同一類物體。但是這種方法對所有單詞都是同樣對待，正常的做法是采用TF-IDF(term frequency-inverse document frequency)

IDF（Inverse Document Frequency）：描述單詞在字典中出現的頻率（建構字典時），越低越具有代表性

n為所有描述子數， n_i為該單詞出現次數。ln的作用大概是降低量級，畢竟n很大。

TF（Term Frequency）：單詞在單幀圖像中出現的頻率，越高越具有代表性

n為一幀圖像中所有單詞數， n_i為一幀圖像中該單詞出現次數。

是以将一幀圖像轉化為單詞表示時，我們要計算其單詞的權重：

是以一幀圖像A由單詞 w_i，次數n_i以及權重η_i組成。

驗證子產品（回環處理）

回環的判斷也并沒有這麼簡單，含有很多的篩選環節，畢竟錯誤的回環将帶來巨大災難，甯可不要。例如某一位姿附近連續多次（ORB-SLAM中為3次）與曆史中某一位姿附近出現回環才判斷為回環；回環候選幀仍然要比對，比對點足夠才為回環。方式主要有兩種：

（ａ）時間一緻性。正确的回環往往存在時間上的連續性，是以如果之後一段時間内能用同樣的方法找到回環，則認為目前回環是正确的，也叫做順序一緻性限制。

（ｂ）結構一緻性校驗。對回環檢測到的兩幀進行特征比對并估計相機運動，因為各個特征點在空間中的位置是唯一不變的，與之前的估計誤差比較大小。

然後下面主要就會涉及如何對找到的回環進行優化了。

2. SLAM優化

閉環糾正

在判斷出現回環後，兩幀計算Sim3變換（因為有尺度漂移），也就是從曆史幀直接推算目前位姿。下面我們以ORB-SLAM2為代表減少形成閉環時的Sim3優化。

單目SLAM一般都會發生尺度(scale)漂移，是以Sim3上的優化是必要的。相對于SE3，Sim3的自由度要多一個，而且優化的目标是矯正尺度因子，是以優化并沒有加入更多的變量。

//BRIEF 形成閉環時進行Sim3優化
//Sim3用于回環檢測時計算關鍵幀與回環幀之間的關系 R 、t、 s
//相似變換群Sim3
int Optimizer::OptimizeSim3(KeyFrame *pKF1, //關鍵幀1
                            KeyFrame *pKF2, //關鍵幀2
                            vector<MapPoint *> &vpMatches1,//兩幀比對關系 
                            g2o::Sim3 &g2oS12, //兩個關鍵幀間的Sim3變換
                            const float th2, //核函數門檻值
                            const bool bFixScale//是否進行尺度優化
                            )
{
    //建立優化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType * linearSolver;
 
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
 
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    //使用LM算法
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);
 
    // Calibration
    //得到兩幀相機内參
    const cv::Mat &K1 = pKF1->mK;
    const cv::Mat &K2 = pKF2->mK;
 
    // Camera poses
    //得到兩幀的旋轉與平移位姿
    const cv::Mat R1w = pKF1->GetRotation();
    const cv::Mat t1w = pKF1->GetTranslation();
    const cv::Mat R2w = pKF2->GetRotation();
    const cv::Mat t2w = pKF2->GetTranslation();
 
    // Set Sim3 vertex
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    
    vSim3->_fix_scale=bFixScale;
    //兩幀之間的位姿變化為待估計量
    vSim3->setEstimate(g2oS12);
    vSim3->setId(0);
    //不進行固定參數
    vSim3->setFixed(false);
    //提取相機内參參數
    vSim3->_principle_point1[0] = K1.at<float>(0,2);
    vSim3->_principle_point1[1] = K1.at<float>(1,2);
    vSim3->_focal_length1[0] = K1.at<float>(0,0);
    vSim3->_focal_length1[1] = K1.at<float>(1,1);
    vSim3->_principle_point2[0] = K2.at<float>(0,2);
    vSim3->_principle_point2[1] = K2.at<float>(1,2);
    vSim3->_focal_length2[0] = K2.at<float>(0,0);
    vSim3->_focal_length2[1] = K2.at<float>(1,1);
    //加入圖優化頂點
    optimizer.addVertex(vSim3);
 
    // Set MapPoint vertices
    //建立地圖點 頂點
    const int N = vpMatches1.size();
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
    //定義兩幀的公共地圖點
    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
    //定義邊的尺寸
    vector<size_t> vnIndexEdge;
 
    //配置設定空間
    vnIndexEdge.reserve(2*N);
    vpEdges12.reserve(2*N);
    vpEdges21.reserve(2*N);
 
    const float deltaHuber = sqrt(th2);
 
    int nCorrespondences = 0;
    //周遊所有比對點
    for(int i=0; i<N; i++)
    {
        if(!vpMatches1[i])
            continue;
 
        MapPoint* pMP1 = vpMapPoints1[i];
        MapPoint* pMP2 = vpMatches1[i];
 
        const int id1 = 2*i+1;
        const int id2 = 2*(i+1);
 
        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);
        //如果比對的兩幀看到的地圖點都存在
        if(pMP1 && pMP2)
        {
            //判斷是不是好點
            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
            {
                //建立優化器 優化位姿1
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
                //提取世界坐标轉化相機坐标
                cv::Mat P3D1w = pMP1->GetWorldPos();
                cv::Mat P3D1c = R1w*P3D1w + t1w;
                //待優化為相機位姿
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));
                vPoint1->setId(id1);
                //固定原點坐标
                vPoint1->setFixed(true);
                optimizer.addVertex(vPoint1);
                //優化位姿2
                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D2w = pMP2->GetWorldPos();
                cv::Mat P3D2c = R2w*P3D2w + t2w;
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));
                vPoint2->setId(id2);
                vPoint2->setFixed(true);
                optimizer.addVertex(vPoint2);
            }
            else
                continue;
        }
        else
            continue;
        //成功一個點加一個
        nCorrespondences++;
 
        // Set edge x1 = S12*X2
        //建立邊，就是計算重投影誤差
        //提取像素坐标
        Eigen::Matrix<double,2,1> obs1;
        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
        obs1 << kpUn1.pt.x, kpUn1.pt.y;
 
        //建立邊，也就是誤差
        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
        //連接配接的兩個頂點
        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        //觀測值
        e12->setMeasurement(obs1);
        //資訊矩陣
        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);
        //魯棒核
        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
        e12->setRobustKernel(rk1);
        rk1->setDelta(deltaHuber);
        //添加邊
        optimizer.addEdge(e12);
 
        // Set edge x2 = S21*X1
        //反向投影在進行優化
        Eigen::Matrix<double,2,1> obs2;
        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
        obs2 << kpUn2.pt.x, kpUn2.pt.y;
 
        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
 
        e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
        e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e21->setMeasurement(obs2);
        float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
        e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
 
        g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
        e21->setRobustKernel(rk2);
        rk2->setDelta(deltaHuber);
        optimizer.addEdge(e21);
 
        vpEdges12.push_back(e12);
        vpEdges21.push_back(e21);
        vnIndexEdge.push_back(i);
    }
 
    // Optimize!
    //開始優化
    optimizer.initializeOptimization();
    optimizer.optimize(5);
 
    // Check inliers
    //清除外點和誤差過大的點
    int nBad=0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;
 
        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
            optimizer.removeEdge(e12);
            optimizer.removeEdge(e21);
            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
            nBad++;
        }
    }
    //确定疊代次數
    int nMoreIterations;
    if(nBad>0)
        nMoreIterations=10;
    else
        nMoreIterations=5;
 
    if(nCorrespondences-nBad<10)
        return 0;
 
    // Optimize again only with inliers
    //再次優化，隻優化内點
    optimizer.initializeOptimization();
    optimizer.optimize(nMoreIterations);
 
    int nIn = 0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;
 
        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        }
        else
            nIn++;
    }
 
    // Recover optimized Sim3
    //恢複出位姿變化關系
    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
    g2oS12= vSim3_recov->estimate();
 
    return nIn;
}      

當我們用第m幀推算了回環幀n的位姿時，使得n的位姿漂移誤差較小，但其實同時可以用第n幀來計算n-1幀的位姿，使n-1幀的位姿漂移誤差也減小。是以，這裡還要有一個位姿傳播。

另外我們可以優化所有的位姿，也就是進行一個位姿圖優化（由位姿變換建構位姿限制）。

最後，我們還可以進行一起全局所有變量的BA優化。

//  初始化
g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver
     = new g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >();
g2o::BlockSolver_6_3* blockSolver = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver
= new g2o::OptimizationAlgorithmLevenberg( blockSolver );    
optimizer.setAlgorithm( solver );
optimizer.setVerbose( false );
 
// 初始頂點    
g2o::VertexSE3* v = new g2o::VertexSE3();
   v->setId( cutIndex );
   v->setEstimate( Eigen::Isometry3d::Identity() ); 
   v->setFixed( true ); 
   optimizer.addVertex( v )
// 添加新的節點（頂點）
g2o::VertexSE3* vNext = new g2o::VertexSE3();
vNext->setId(currFrame.frameID);
vNext->setEstimate( Eigen::Isometry3d::Identity());
optimizer.addVertex(vNext);
 
// 添加邊的資料
g2o::EdgeSE3* edgeNew = new g2o::EdgeSE3();
edgeNew->setVertex(0, optimizer.vertex(pousFrame.frameID));
 
edgeNew->setVertex(1, optimizer.vertex(currFrame.frameID));
 
edgeNew->setRobustKernel( new g2o::RobustKernelHuber() );
//edgeNew->setInformation( Eigen::Matrix2d::Identity());  //the size is worthy to research
// 資訊矩陣
 
// 兩個節點的轉換矩陣
Eigen::Isometry3d T = cvMat2Eigen( m_result.rotaMatrix, m_result.tranMatrix );    
edgeNew->setMeasurement(T);
optimizer.addEdge(edgeNew);
// 開始優化
optimizer.initializeOptimization();
optimizer.optimize(10);      

3. 參考連結

​​https://baijiahao.baidu.com/s?id=1627227355343777871&wfr=spider&for=pc