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

SimpleTriangle發表於2021-12-02

背景

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

全域性優化前_底.png圖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的使用方法基本就是:

  1. 宣告一個優化器;
  2. 選擇求解方法;
  3. 構造圖--頂點和邊的關係;
  4. 優化處理。

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

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

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

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

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

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

拆解gicp_demo

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

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

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

再次,為圖新增了1000條邊。在此過程中,首先根據節點id獲取邊所需要連結的兩個頂點(節點),vp0vp1,並基於true_points "製作" 了包含噪聲的兩個三維點pt0pt1(這一步其實已經預設pt0pt1為對應點對);然後 宣告瞭一個Edge_V_V_GICP型別的圖邊結構,該邊是一個g2o的二元邊(class G2O_TYPES_ICP_API Edge_V_V_GICP : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>),該二元邊分別連結vp0vp1;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:

aa
圖 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片粗配後的散亂點雲,想要對全域性點雲進行全域性優化配準。

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

  1. 相鄰兩片點雲之間具有較高重疊率;
  2. 相隔(至少一塊點雲)點雲之間有或沒有重疊;
  3. 每塊點雲可以有一個、零個(這個條件可以存在,構造g2o圖時與條件1並不衝突)或者多個高重疊率的點雲;
  4. 點雲之間的重疊關係是對應的(即: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之間最優變換。

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

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

  1. 計算全域性點雲相互之間的對應點對與重疊率;
  2. 重疊點雲篩選(重疊率較低的點雲認為無重疊,不參與g2o圖中邊的構建);
  3. g2o全域性構圖:

    • 圖節點
    • (重點)
    • 優化器與優化演算法
  4. 基於步驟3的輸出矩陣,更新全部點雲的座標;
  5. 重複步驟1-4,直到滿足終止條件。

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

程式實現

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

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

點雲資料結構

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

  1. 點集;
  2. 是否固定;
  3. 變換矩陣;
  4. 鄰接資訊;
  5. 必要方法(最近鄰點雲、對應點對計算)。

構造如下:

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學習過程,在這裡所謂二元超圖,只單純表示圖中每個節點有兩個關係節點,同樣,每個節點只有兩個約束關係),最終結果如下:

全域性優化後_底.png
圖4 點雲底面
全域性優化後_耳朵_消除錯位.png
圖5 區域性細節

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

執行過程中截圖如下:

第1次.png 圖 6 第7次.png 圖 7 第18次.png 圖 8 第40次.png 圖 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左右的對應點對),逐步減小,也就是說對應點對之間的全域性歐式距離平方和在逐步減小,同樣符合實際情況。

該二元超圖結構如下:

1638338902(1).jpg 圖10 二元圖

測試二

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

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

san1.png san2.png

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

微信圖片_20211201103100.jpg

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

總結&題外話

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

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

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

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

總之,我知道:g2o可以用來解決非線性優化問題

囉裡囉唆......

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

相關文章