背景

自己手頭有一堆經過初始配準後的三維散亂點雲，經過粗配後，全部點雲基本統一在了同一個座標系下，如圖1和圖2所示。為了獲取更好的全域性結果，需要對粗配後的點雲進行全域性配準優化。

圖1 點雲底面

圖2 區域性細節

上述點雲是基於pcl進行顯示，不同的顏色代表不同的點雲塊，從圖1可以明顯看出，全域性點雲"融合"(其實並未融合，只是全部載入顯示)，效果較差，從圖2可以看出，針對[耳朵]部分出現較大錯位。

因為自己之前對G2O多多少少有一點了解，但是也並沒有進行過多深入的研究。知道G2O可以用來解決全域性優化問題，正好和自己要求解的問題非常非常相似。

因此，便毅然決然的選擇使用G2O來針對自己的問題構建解決方案。

G2O的進一步瞭解

G2O是將非線性優化和圖論相結合的產物，大多時候用於解決slam後端優化問題，其自身攜帶的例子大多也與slam有關。g2o中所謂的圖，就是一堆節點和邊按照一定的關係連結而成。其中，節點是要優化的目標，邊是各個優化目標之間的關係(也稱其誤差項，在這裡自己更喜歡稱為【關係】)。

基於CMake + VS2017 完成G2O庫的安裝，安裝過程沒有做詳細記錄，基本百度能夠解決。

安裝完g2o後，按照習慣去翻看其自身原始碼中所攜帶的example，以便尋找靈感，在example目錄中一眼便看中了【gicp_demo】。

G2O--gicp_demo

g2o的使用方法基本就是：

宣告一個優化器；
選擇求解方法；
構造圖--頂點和邊的關係；
優化處理。

初次看到g2o的gicp時，自認為“該gicp方法是全域性(global)的”，然而事實並非如此，事實上，其甚至不能稱為是一個完整的icp問題

(關於上述紅色字型的表示，目前僅僅是個人理解，或許有錯誤，也請多多留言指正，一起交流學習)

我們知道，ICP求解是一個迭代計算過程，經典ICP求解的主要步驟為：

輸入兩片點雲AB，求解對應點對(三維模型至少3個不共線的點)；
基於對應點對，構造A到B的變換矩陣M；
將M作用於A，得到A*，並用A*代替A；
迭代終止條件(迭代次數或者最小誤差度量)；
重複步驟1--3，直到滿足步驟4，終止。
輸出變換矩陣M。

但是細看g2o的gicp_demo，其流程並不如經典ICP求解過程一樣，而更像一個ICP中步驟2的求解問題。

再來深入看看g2o中給出的gicp_demo。

拆解gicp_demo

首先還是直接先把g2o官方例子貼出來吧(雖然非常討厭直接貼別人程式碼)，便於說明。

在該Demo中，g2o首先宣告並設定了優化器optimizer，並製作了包含1000個點的集合true_points作為源點雲。

其次，為圖新增了兩個節點並設定節點ID。這裡的節點型別為VertexSE3（ class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3>），也是主要的優化目標。依據節點新增到圖中的順序，將第一次新增的節點視為固定視角；vc->setEstimate(cam);該程式碼段告訴我們，真正求解的結果是儲存在Eigen::Isometry3d型別的相機位姿(本質上是一個矩陣)，這裡cam引數的型別是Eigen::Isometry3d；這一步其實只是宣告瞭兩個空節點，節點引數只是單位Eigen::Isometry3d。

再次，為圖新增了1000條邊。在此過程中，首先根據節點id獲取邊所需要連結的兩個頂點(節點)，vp0和vp1,並基於true_points "製作" 了包含噪聲的兩個三維點pt0和 pt1（這一步其實已經預設pt0和 pt1為對應點對）；然後宣告瞭一個Edge_V_V_GICP型別的圖邊結構，該邊是一個g2o的二元邊(class G2O_TYPES_ICP_API Edge_V_V_GICP : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>)，該二元邊分別連結vp0和vp1；g2o還提供了EdgeGICP型別作為觀測值，EdgeGICP型別可以存放對應點對。在該步驟中，一定要非常注意節點和三維座標點的所屬--對應關係。至此，基本能夠將所有資訊放入g2o的圖中，該步驟主要關心的在於如何將自己的三維點對放入到g2o圖中。

最後，初始化圖關係並進行了N次迭代優化，每個節點的優化結果儲存在optimizer.vertices()返回值型別的雜湊表中，該雜湊表中：鍵--對應節點id，值--對應節點，這裡為VertexSE3型別，從VertexSE3獲得的Eigen::Isometry3d型別是我們真正關心的結果資料。
該示例應該構建瞭如下一張超圖，其中圖有兩個圖節點，n1為固定節點，n2為變動的節點，節點之間有1000條邊，每條邊連結一對對應點，針對ICP問題，對應點中的固定點掛接圖節點n1，變動的點掛接圖節點n2：

節點--邊

void icp() {
    double euc_noise = 0.01;       // noise in position, m

    //宣告優化器
    SparseOptimizer optimizer;
    //是否列印詳細資訊
    optimizer.setVerbose(false);
    
    // variable-size block solver
    //設定一個求解方法
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));

        /*g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(
            g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));*/

    //設定優化器使用的優化方法
    optimizer.setAlgorithm(solver);

    //隨機點座標
    vector<Vector3d> true_points;
    for (size_t i = 0; i < 1000; ++i)
    {
        //這裡從均勻分佈中取樣了一組數字
        true_points.push_back(Vector3d((g2o::Sampler::uniformRand(0., 1.) - 0.5) * 3,
            g2o::Sampler::uniformRand(0., 1.) - 0.5,
            g2o::Sampler::uniformRand(0., 1.) + 10));
    }

    // set up two poses
    //猜測: 設定兩個相機位姿
    int vertex_id = 0;
    for (size_t i = 0; i < 2; ++i)
    {
        // set up rotation and translation for this node
        //平移向量
        Vector3d t(0, 0, i);
        //四元數旋轉
        Quaterniond q;
        q.setIdentity();

        //李群(特殊尤拉群，包含旋轉和平移，自己感覺和4*4矩陣類似)
        Eigen::Isometry3d cam; // camera pose
        cam = q;
        //返回平移向量的可寫表示式
        cam.translation() = t;

        // set up node
        //李群。這裡作為節點,作為優化變數
        VertexSE3 *vc = new VertexSE3();
        //設定頂點的估計值
        vc->setEstimate(cam);
        //設定該節點在 圖 中的id，以便追蹤
        vc->setId(vertex_id);      // vertex id

        //列印節點的初始平移和旋轉矩陣
        cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;

        // set first cam pose fixed
        if (i == 0)
            vc->setFixed(true);

        // add to optimizer
        //優化器新增節點
        optimizer.addVertex(vc);

        vertex_id++;
    }

    // set up point matches
    for (size_t i = 0; i < true_points.size(); ++i)
    {
        // get two poses
        //獲取前述新增的節點。
        /* optimizer.vertices()的返回值是一個雜湊表(Map)型別，本質是std::unordered_map<int, Vertex*>，
        */
        VertexSE3* vp0 =
            dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
        VertexSE3* vp1 =
            dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

        // calculate the relative 3D position of the point
        Vector3d pt0, pt1;
        pt0 = vp0->estimate().inverse() * true_points[i];
        pt1 = vp1->estimate().inverse() * true_points[i];

        // add in noise
        //新增高斯噪聲
        pt0 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise));

        pt1 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise));

        // form edge, with normals in varioius positions
        Vector3d nm0, nm1;
        nm0 << 0, i, 1;
        nm1 << 0, i, 1;
        nm0.normalize();
        nm1.normalize();

        //g20的二元邊
        Edge_V_V_GICP * e           // new edge with correct cohort for caching
            = new Edge_V_V_GICP();

        e->setVertex(0, vp0);      // first viewpoint
        e->setVertex(1, vp1);      // second viewpoint

        EdgeGICP meas;
        meas.pos0 = pt0;
        meas.pos1 = pt1;
        meas.normal0 = nm0;
        meas.normal1 = nm1;

        //定義觀測值
        e->setMeasurement(meas);
        //        e->inverseMeasurement().pos() = -kp;

        meas = e->measurement();
        // use this for point-plane
        //約束資訊(協方差矩陣的逆) = 點面的精度矩陣資訊
        e->information() = meas.prec0(0.01);

        optimizer.addEdge(e);
    }

    // move second cam off of its true position
    //變換第二個點雲。
    VertexSE3* vc =
        dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
    Eigen::Isometry3d cam = vc->estimate();
    cam.translation() = Vector3d(0, 0, 0.2);
    vc->setEstimate(cam);

    //初始化整個圖結構
    optimizer.initializeOptimization();
    //計算所有邊的誤差向量
    optimizer.computeActiveErrors();

    //輸出優化前的誤差平方
    cout << "Initial chi2(before opt) = " << FIXED(optimizer.chi2()) << endl;

    optimizer.setVerbose(true);

    optimizer.optimize(5);

    //輸出優化前的誤差平方
    cout << "Initial chi2(after opt) = " << FIXED(optimizer.chi2()) << endl;

    //列印變化矩陣
    cout << endl << "Second vertex should be near 0,0,1" << endl;
    cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
        ->estimate().translation().transpose() << endl;
    cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
        ->estimate().translation().transpose() << endl;
}

測試

執行g2o自帶的上述例子，最終能夠列印出變換矩陣。然後將該例子單獨摘出來，換入自己的資料(對應點對)，也能輸出變換矩陣，然後將變換矩陣作用於點雲 ,結果如圖3：

圖 3 g2o--gicp

可能早有大神預料到會是如上結果！！不得不說，g2o的優化結果也太差強人意......

真是這樣嗎？ ......

小結

如果按照上述流程，依照g2o的官方例子，結果真是那樣！！！但是和pcl的icp對比，結果是真的差，問題出在哪裡？

問題出在上文中紅字部分，這裡依然用紅字提醒自己---g2o的gicp並不能算完全的ICP求解方案。

通過分析官方程式碼例子可以發現，其在求解前，本質上已經預設了輸入點對是對應的，在此基礎上進行迭代計算，本質是依據同一組對應點對，迭代計算該組對應點之間的最優變換矩陣，對於整體兩片點雲來說，這樣其實只是完成了一次icp計算，結果當然不理想。

換句話說，該Demo遺漏(或者g2o本就如此設計，或是自己瞭解不夠)ICP迭代方案中對應點對的計算過程，也就是缺少了步驟1，g2o--gicp只是單純的計算了步驟2，只得到了單次的最優變換矩陣。對於整體兩片點雲的icp求解問題，在進行第2次求解時，對應點對的對應關係已經發生了變化，因此g2o---gicp_demo得到的矩陣只是單次的最優矩陣，所以結果也就如圖3所示。

那麼如何得到整體最優結果呢？當然是將上述步驟放入大迴圈中，每計算完一次g2o--gicp，變換點雲，重新求解對應關係，依次迭代計算。結果圖省略...

構建優化圖

在充分理解了g2o自帶icp例子後，回到最初自己要求解的問題。有N片粗配後的散亂點雲，想要對全域性點雲進行全域性優化配準。

所謂粗配後的全域性點雲，具有以下特點：

相鄰兩片點雲之間具有較高重疊率；
相隔(至少一塊點雲)點雲之間有或沒有重疊；
每塊點雲可以有一個、零個(這個條件可以存在，構造g2o圖時與條件1並不衝突)或者多個高重疊率的點雲；
點雲之間的重疊關係是對應的(即：A與B 、C、D重疊，那麼BCD的重疊點雲中一定也有A)。

優化目標

上述談到的優化目標只是我們感性上的認識，但是還必須要將優化目標轉化為數學表達。口述如下：優化目標 = 求解--N片三維散亂點雲，以點雲A為目標點雲，計算所有三維散亂點雲配準到A的變換矩陣，該變換矩陣使得所有對應點對的歐式距離取得最小，或者達到指定的迭代變換次數。

上述優化目標隱含：若A與C沒有重疊，則C無法直接向A進行配準對齊，但是A與B，B與C有重疊，則C變換到A則需要先變換到B，由B的矩陣再變化到A，而且要保證A與B，B與C均為最優變換，換句話來說，就是B最優變換到A，C最優變換到B，則完成了A B C之間最優變換。

確認優化目標的數學表達後，還需要確認點雲間的重疊率表達，口述如下：重疊率 = 兩兩點雲對應點對的數量與該兩片點雲平均點數的比值。

完成上述兩個數學定義後，程式的總體流程應該如下：

計算全域性點雲相互之間的對應點對與重疊率；
重疊點雲篩選(重疊率較低的點雲認為無重疊，不參與g2o圖中邊的構建)；
g2o全域性構圖：
- 圖節點
- 邊（重點）
- 優化器與優化演算法
基於步驟3的輸出矩陣，更新全部點雲的座標；
重複步驟1-4，直到滿足終止條件。

最終輸出為優化後的全域性點雲及對應的變換矩陣。

程式實現

工程中使用了PCL點雲庫和g2o(廢話)，其中pcl主要用於計算點雲重疊率。

通過上述【g2o--gicp_demo】可知，g2o中已經為ICP方案提供了定義好的圖節點類VertexSE3和圖二元邊類Edge_V_V_GICP 以及邊類EdgeGICP，這裡直接拿來使用，省去了自定義圖邊、圖節點麻煩(當然，若深入學習g2o，還是建議多做更多探索)。

點雲資料結構

通過前述分析，可知，某點雲結構應該包含如下必要內容：

點集；
是否固定；
變換矩陣；
鄰接資訊；
必要方法(最近鄰點雲、對應點對計算)。

構造如下：

typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;

class MyPnts
{
public:
    MyPnts() ;
    ~MyPnts();
    int id;
    int v_number;
    
    vector<Vector3d> pts;

    bool fixed = false;
    
    Isometry3d pose; //該點雲的初始位姿，也是優化目標
    
    //該點雲的所有鄰接資訊
    vector<OutgoingEdge*> neighbours;
    
    //當前點雲在所有點雲序列中的k個最近鄰點雲(重疊率最高的前K個)
    void computeKNNearPnts(vector< std::shared_ptr<MyPnts>>& frames, int k);
    
    //對應點對計算
    void computeClosestPointsToNeighbours(vector< shared_ptr<MyPnts>>& frames, float thresh);
    
    /*
    * Method:    calCorrespond  計算兩片點雲的相互對應點對的索引
    * Access:    public
    * Returns:   std::unordered_map<int, std::vector<int>> first 0->src,1 -> tar;
    * Parameter: pointcloud::Ptr src
    * Parameter: pointcloud::Ptr tar
    */
    std::unordered_map<int, std::vector<int>> calCorrespond(MyPnts src, MyPnts tar,double dst = 10.0);

private:
};

其中，計算當前點雲與全域性其他點雲(除當前點雲外)重疊關係、對應點對均在computeKNNearPnts()函式中實現。

造圖

這是最重要也是及其容易出錯的地方。

前述 [粗配後全域性點雲的特點]，決定了g2o中圖結構的特點，圖邊應該滿足 [粗配後全域性點雲的特點] 的描述，偽圖如下：

全域性icp 偽圖

如上圖所示，假設：全域性有5片點雲，則g2o圖有5個節點，n1為固定節點，n1和n2有重疊，n2和n5有重疊，n1和n5也有重疊，但n1和n3 、n1 和 n4、 n2和n4等幾個節點之間不存在重疊關係。

注意：這裡的重疊關係，在g2o看來是約束關係，進一步的，是兩片點雲之間的邊的連結關係，此邊可能有成百上千個對應點對構成。上圖中的帶箭頭的邊僅表示示意，並非真正的圖邊。

在清楚了圖結構後，構造圖程式碼如下：

void G2oPCL::global_icp2(std::vector<std::shared_ptr<MyPnts>> &pnts)
{
    using namespace g2o;
    using namespace std;
    using namespace Eigen;
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(false);
    g2o::OptimizationAlgorithmLevenberg* solver = new                         g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<g2o::BlockSolverX>(g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));
optimizer.setAlgorithm(solver);

//節點
for (int i = 0; i < pnts.size(); ++i) {

    g2o::VertexSE3 *vc = new VertexSE3();

    vc->setEstimate( (*pnts[i]).pose);
    vc->setId(i);      // vertex id
    if (i == 0) {
        vc->setFixed(true);
        (*pnts[i]).fixed = true;
    }
    optimizer.addVertex(vc);
}

//構造全域性邊關係
for (int i = 0; i < pnts.size(); ++i) {
    std::shared_ptr<MyPnts> &current = pnts[i];
    int current_nebor = current->neighbours.size();
    for (int j = 0; j < current_nebor; ++j) {
        OutgoingEdge *oe = current->neighbours[j];
        int nearIdx = oe->neighbourIdx;
        std::shared_ptr<MyPnts> &dst = pnts[nearIdx];
        g2o::VertexSE3* vp0 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(nearIdx)->second); //dstCloud
        g2o::VertexSE3* vp1 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(current->id)->second); //srcCloud
        for (auto cor : oe->correspondances) {
            g2o::Edge_V_V_GICP * e = new g2o::Edge_V_V_GICP();
            e->setVertex(0, vp0);      
            e->setVertex(1, vp1);   
            g2o::EdgeGICP meas;
            meas.pos0 = dst->pts[cor.second];
            meas.pos1 = current->pts[cor.first];

            e->setMeasurement(meas);
            //use this for point-point
            e->information().setIdentity();
            
            optimizer.addEdge(e);    
        }    
    }
}

optimizer.initializeOptimization();

optimizer.computeActiveErrors();
double chiInit = optimizer.chi2(); //stop innerround if we get 100% better
    
optimizer.optimize(100);
cout << "round: " << "s" << " chi2: " << FIXED(chiInit) << endl;
for (int i = 0; i < pnts.size(); ++i) {
    VertexSE3 *res = dynamic_cast<VertexSE3*>(optimizer.vertices().find(i)->second);

    Isometry3d transAndRot = res->estimate();

    MyPnts &mypnts = *pnts[i];

    for (int j = 0; j < mypnts.v_number; ++j) {
            mypnts.pts[j] = transAndRot * mypnts.pts[j];
        }
    }
}

在上述程式碼中，不僅構造了圖結構，設定了各個節點之間的相互關係，而且還更新了點的座標，為方便下次迭代中計算對應點對提供了方便。

至此，基於g2o解決開始提到的問題框架基本完成，最終主程式程式碼如下：

int main()
{
    std::unique_ptr<G2oPCL> gp = std::make_unique<G2oPCL>();
    
    clock_t begin, end;
    begin = clock();

    std::vector<shared_ptr<MyPnts>> pnts;
    loadPnts(pnts, "");

    //計算與目標點雲重疊率最高的兩片點雲
    computeNumbers(pnts, 2);

    int N = 40;
    
    for (int i = 0; i <N ; i++) {
    
        std::cout << "\n===這是第" << i << "次全域性優化===\n" << std::endl;
        //對應點對約束距離為5.0
        computeClosestPoints(pnts, 5.0);
        gp->global_icp2(pnts);
    
    }
    end = clock();
    double t = (end - begin) / 1000.0;
    std::cout << "\n\n 執行時間是：" << t << std::endl;

    std::cout << "儲存結果到本地" << std::endl;
    savePnts(pnts);
}

實驗測試

完成上述準備與程式碼程式設計後，將自己的資料（圖1和圖2所示點雲），輸入優化系統中。

測試一

在此次測試中，computeNumbers(pnts, 2)第二個引數為2，也就是構建了一個二元超圖（所謂二元超圖，只單純是自己的定義，完全不與其他任何g2o程式或程式碼或教程相符合，也不具備真實數學意義，同樣不適用於其他g2o學習過程，在這裡所謂二元超圖，只單純表示圖中每個節點有兩個關係節點，同樣，每個節點只有兩個約束關係），最終結果如下：

圖4 點雲底面

圖5 區域性細節

將圖4 圖5分別與圖1 圖2進行對比，肉眼可見效果提升了很多。

執行過程中截圖如下：

圖 6

圖 7

圖 8

圖 9

觀察圖6--圖9，因為在computeNumbers(pnts, 2);傳入的引數為2，所以這裡只計算了每塊點雲重疊率最高的兩塊，列印資訊可是看出，id為0的點雲重疊率最高的是id為1和id為2的，id為3的點雲重疊率最高的是id為2和id為4的，也就是說，當前帶點雲重疊率最高的是其兩片相鄰的點雲，這是符合自己的實際情況的；繼續看，隨著迭代過程次數的增加，每塊點雲之間的重疊率是不斷變化的，這也同樣符合實際情況；最後，看誤差引數估計ch2的輸出變化 62w-->24w-->8w-->5w(全域性大概20w左右的對應點對)，逐步減小，也就是說對應點對之間的全域性歐式距離平方和在逐步減小，同樣符合實際情況。

該二元超圖結構如下：

圖10 二元圖

測試二

設定computeNumbers(pnts, N);引數N為3時，構建一張三元超圖。

在三元超圖下，最終效果和圖4 圖5類似，同樣認為完成了全域性優化；期間，程式執行過程中資訊輸出如下：

根據上述列印資訊可知，構造的三元圖結構如下所示：

至此，基本完成了自己最初的目的，欣喜....

總結&題外話

自己目前也只是g2o新手村普通村民，且上述描述也並非真正意義上的slam問題，所述描述中只是自己針對所面臨問題探求思路，不具備教學性、更不具權威性(有些誇大)，僅作個人記錄與興趣交流。

上述描述的求解方案和思路，應該可以用來求解散亂點雲多視角全域性配準優化問題，而且也應該算是一個通用(不僅僅限於相鄰點雲重疊)的求解方案。

G2O在slam後端中使用的比較多，官方提供的demo大多也是slam2d、slam3d等，但又不僅限於此。因為針對自己所面臨的問題，並沒有將其向slam方向深入擴充套件，所以也並沒有從其他教程所述那樣，從slam2d例程入手，而是選擇了自己較為熟悉的ICP領域。

不過話又說回來，自己所面的問題從整體解決思路上看，又可以作為是一個典型的slam優化問題：已知多個視角閉環的三維點雲，通過求取路標點，求解全域性相機位姿。

總之，我知道：g2o可以用來解決非線性優化問題。

囉裡囉唆......

(順便吐槽圖片排版是真爛)

G2O與多視角點雲全域性配準優化

背景